#include "pipe_merge.h"
#include <sys/time.h>
#include <unistd.h>
#include <stdio.h>
#include <linux/fb.h>
#include <sys/mman.h>
#include<sys/ioctl.h>

#include <linux/fb.h>
#include <fcntl.h>
#include <stdbool.h>
#include <sys/prctl.h>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include<atomic>

#include"v4l2_drv/v4l2_capture.h"
#include"drm_drv/drm_plane_drv.h"


//#include <QDebug>


typedef unsigned char byte;


//#define WIGHT  704
//#define HEIGHT  290
#define WIGHT  1280
#define HEIGHT  720

using namespace cv;
using namespace std;


//色相
#define H_MIN   0
#define H_MAX   180
//饱和度
#define S_MIN  0
#define S_MAX  255
//亮度
#define V_MAX  255

static unsigned char buffer[1280*720*3];

static Mat inf_buffer(720,1280,CV_8UC3);




void Pipe_Merge::setMergemode(Pipe_MERGEMODE mode)
{
    mode_mutex.lock();
    g_mergeMode = mode;
    mode_mutex.unlock();
}

Pipe_MERGEMODE Pipe_Merge::getMergemode()
{
    Pipe_MERGEMODE tmp;
    mode_mutex.lock();
     tmp = g_mergeMode;
     mode_mutex.unlock();
    return tmp;
}

void Pipe_Merge::setColormode(bool switch_flag)
{
    mode_mutex.lock();
     gRedColorSwitch = switch_flag;
     mode_mutex.unlock();
}
bool Pipe_Merge::getColormode()
{
    bool tmp=true;
    mode_mutex.lock();
    tmp=gRedColorSwitch;
    mode_mutex.unlock();
    return tmp;
}

void Pipe_Merge::setMergeposition(Pipe_MixPositionItem position)
{
    g_mixPosition = position;
}
Pipe_MixPositionItem Pipe_Merge::getMergeposition()
{
    return g_mixPosition;
}

void format_convert(char *databuff, int datalen)
{
    cv::Mat yuv1(HEIGHT*3/2, WIGHT, CV_8UC1);
    memcpy(yuv1.data,databuff, datalen);
    cv::Mat rgb;
    cvtColor(yuv1, rgb, CV_YUV2RGB_NV12);
}

static void yuvI420ToNV12(uchar *I420,int w,int h,uchar *NV12)
{
    memcpy(NV12,I420,w*h);//y分量
    for(int i = 0,j = 0;i<w*h/4;i++,j+=2)
    {
         memcpy(NV12+w*h+j,I420+w*h+i,1);//u分量
         memcpy(NV12+w*h+j+1,I420+w*h+i+w*h/4,1);//v分量
    }
}
void Flip_image(unsigned char *srcbuff, unsigned char *dstbuff, int datalen)
{
    cv::Mat yuv1(HEIGHT*3/2, WIGHT, CV_8UC1);
    cv::Mat rgb;
    cv::Mat flip1;
    cv::Mat yuv2;
    cv::Mat flip2(HEIGHT*3/2, WIGHT, CV_8UC1);

    memcpy(yuv1.data,srcbuff, datalen);
    cvtColor(yuv1, rgb, CV_YUV2RGB_NV12);
    flip(rgb, flip1, 1);
    cvtColor(flip1, yuv2, CV_RGB2YUV_I420);
    yuvI420ToNV12(yuv2.data,WIGHT,HEIGHT,flip2.data);
    memcpy(dstbuff, flip2.data, datalen);
}

Pipe_CameraBriCon g_cameraBriCon;

void setCameraContrast(Pipe_CameraBriCon cameraBrightnessContrast)
{
    char pcCMD0[64];
    char pcCMD1[64];
    if(cameraBrightnessContrast.fcContrast != g_cameraBriCon.fcContrast)
    {
        g_cameraBriCon.fcContrast = cameraBrightnessContrast.fcContrast;
        sprintf(pcCMD0, "i2ctransfer -f -y 1 w2@0x31 0x25 %d", g_cameraBriCon.fcContrast);
        system(pcCMD0);
        sprintf(pcCMD1, "i2ctransfer -f -y 1 w2@0x31 0x26 %d", g_cameraBriCon.fcContrast);
        system(pcCMD1);
    }
    if(cameraBrightnessContrast.bcContrast != g_cameraBriCon.bcContrast)
    {
        g_cameraBriCon.bcContrast = cameraBrightnessContrast.bcContrast;
        memset(pcCMD0, 0, strlen(pcCMD0));
        sprintf(pcCMD0, "i2ctransfer -f -y 1 w2@0x31 0x24 %d", g_cameraBriCon.bcContrast);
        system(pcCMD0);
    }
    if(cameraBrightnessContrast.lcContrast != g_cameraBriCon.lcContrast)
    {
        g_cameraBriCon.lcContrast = cameraBrightnessContrast.lcContrast;
        memset(pcCMD0, 0, strlen(pcCMD0));
        sprintf(pcCMD0, "i2ctransfer -f -y 1 w2@0x32 0x24 %d", g_cameraBriCon.lcContrast);
        system(pcCMD0);
    }
    if(cameraBrightnessContrast.rcContrast != g_cameraBriCon.rcContrast)
    {
        g_cameraBriCon.rcContrast = cameraBrightnessContrast.rcContrast;
        memset(pcCMD0, 0, strlen(pcCMD0));
        sprintf(pcCMD0, "i2ctransfer -f -y 1 w2@0x32 0x26 %d", g_cameraBriCon.rcContrast);
        system(pcCMD0);
    }
}
void setCameraBrightness(Pipe_CameraBriCon cameraBrightnessContrast)
{
    char pcCMD1[64];
    char pcCMD2[64];
    printf("g_cameraBriCon.rcBrightness[%d] cameraBrightnessContrast.rcBrightness[%d] "
           "line[%d]\n", g_cameraBriCon.rcBrightness,cameraBrightnessContrast.rcBrightness, __LINE__);
   if(cameraBrightnessContrast.fcBrightness != g_cameraBriCon.fcBrightness)
   {
       if(-128<=cameraBrightnessContrast.fcBrightness && cameraBrightnessContrast.fcBrightness<0)
           g_cameraBriCon.fcBrightness = 256+cameraBrightnessContrast.fcBrightness;
       else
           g_cameraBriCon.fcBrightness = cameraBrightnessContrast.fcBrightness;
       sprintf(pcCMD1, "i2ctransfer -f -y 1 w2@0x31 0x21 %d", g_cameraBriCon.fcBrightness);
       system(pcCMD1);
       sprintf(pcCMD2, "i2ctransfer -f -y 1 w2@0x31 0x22 %d", g_cameraBriCon.fcBrightness);
       system(pcCMD2);
   }
   if(cameraBrightnessContrast.lcBrightness != g_cameraBriCon.lcBrightness)
   {
       if(-128<=cameraBrightnessContrast.lcBrightness && cameraBrightnessContrast.lcBrightness<0)
           g_cameraBriCon.lcBrightness = 256+cameraBrightnessContrast.lcBrightness;
       else
           g_cameraBriCon.lcBrightness = cameraBrightnessContrast.lcBrightness;
       //左视video4亮度
       memset(pcCMD1, 0, strlen(pcCMD1));
       sprintf(pcCMD1, "i2ctransfer -f -y 1 w2@0x32 0x20 %d", g_cameraBriCon.lcBrightness);
       system(pcCMD1);
   }
   if(cameraBrightnessContrast.rcBrightness != g_cameraBriCon.rcBrightness)
   {
       if(-128<=cameraBrightnessContrast.rcBrightness && cameraBrightnessContrast.rcBrightness<0)
           g_cameraBriCon.rcBrightness = 256+cameraBrightnessContrast.rcBrightness;
       else
           g_cameraBriCon.rcBrightness = cameraBrightnessContrast.rcBrightness;
       //右视video6亮度
       memset(pcCMD1, 0, strlen(pcCMD1));
       sprintf(pcCMD1, "i2ctransfer -f -y 1 w2@0x32 0x22 %d\n", g_cameraBriCon.rcBrightness);
       printf("g_cameraBriCon.rcBrightness[%d]\n pcCMD1[%s] [%d]\n",
              g_cameraBriCon.rcBrightness, pcCMD1, __LINE__);
       system(pcCMD1);
   }
   if(cameraBrightnessContrast.bcBrightness != g_cameraBriCon.bcBrightness)
   {
       if(-128<=cameraBrightnessContrast.bcBrightness && cameraBrightnessContrast.bcBrightness<0)
           g_cameraBriCon.bcBrightness = 256+cameraBrightnessContrast.bcBrightness;
       else
           g_cameraBriCon.bcBrightness = cameraBrightnessContrast.bcBrightness;
       //后视video0亮度
       sprintf(pcCMD1, "i2ctransfer -f -y 1 w2@0x31 0x20 %d", g_cameraBriCon.bcBrightness);
       system(pcCMD1);
   }
}

#if 0
void draw_redcolor(Mat temImage)
{
    uchar lutData[256 * 3];
    for (int i = 0; i<256; i++)
    {
        if (i <= 90)
        {
            lutData[i * 3] = 0;
            lutData[i * 3 + 1] = 0;
            lutData[i * 3 + 2] = 0;
        }
        if (i > 90 && i <= 120)
        {
            lutData[i * 3] = 200;
            lutData[i * 3 + 1] = 49;
            lutData[i * 3 + 2] = 49;
        }
        if (i > 120 && i <= 150)
        {
            lutData[i * 3] = 205;
            lutData[i * 3 + 1] = 39;
            lutData[i * 3 + 2] = 0;
        }
        if (i > 150 && i <= 200)
        {
            lutData[i * 3] = 214;
            lutData[i * 3 + 1] = 34;
            lutData[i * 3 + 2] = 34;
        }
        if (i > 200)
        {
            lutData[i * 3] = 255;
            lutData[i * 3 + 1] = 0;
            lutData[i * 3 + 2] = 0;
        }
    }
    Mat lut(1, 256, CV_8UC3, lutData);
    LUT(temImage, lut, temImage);
}

void draw_redcolor_hongwai(Mat temImage)
{
    uchar lutData[256 * 3];
    for (int i = 0; i<256; i++)
    {
        if (i <= 130)
        {
            lutData[i * 3] = 0;
            lutData[i * 3 + 1] = 0;
            lutData[i * 3 + 2] = 0;
        }
        else if (i > 130 && i <= 210)
        {
            lutData[i * 3] = 255;
            lutData[i * 3 + 1] = 255;
            lutData[i * 3 + 2] = 255;
        }
        else if (i > 210 && i <= 230)
        {
            lutData[i * 3] = 240;
            lutData[i * 3 + 1] = 128;
            lutData[i * 3 + 2] = 128;
        }

        else if (i > 230)
        {
               lutData[i * 3] = 205;
               lutData[i * 3 + 1] = 38;
               lutData[i * 3 + 2] = 38;
        }
        else
        {
            lutData[i * 3] = 255;
            lutData[i * 3 + 1] = 255;
            lutData[i * 3 + 2] = 255;
        }
    }
    Mat lut(1, 256, CV_8UC3, lutData);
    LUT(temImage, lut, temImage);
}
#else
void draw_redcolor(Mat temImage)
{
    uchar lutData[256 * 3];
    for (int i = 0; i<256; i++)
    {
        if (i <= 90)
        {
            lutData[i * 3] = 0;
            lutData[i * 3 + 1] = 0;
            lutData[i * 3 + 2] = 0;
        }
        if (i > 90 && i <= 120)
        {
            lutData[i * 3] = 200;
            lutData[i * 3 + 1] = 49;
            lutData[i * 3 + 2] = 49;
        }
        if (i > 120 && i <= 150)
        {
            lutData[i * 3] = 205;
            lutData[i * 3 + 1] = 39;
            lutData[i * 3 + 2] = 0;
        }
        if (i > 150 && i <= 200)
        {
            lutData[i * 3] = 214;
            lutData[i * 3 + 1] = 34;
            lutData[i * 3 + 2] = 34;
        }
        if (i > 200)
        {
            lutData[i * 3] = 255;
            lutData[i * 3 + 1] = 0;
            lutData[i * 3 + 2] = 0;
        }
    }
    Mat lut(1, 256, CV_8UC3, lutData);
    LUT(temImage, lut, temImage);
}

void draw_redcolor_hongwai(Mat temImage)
{
    uchar lutData[256 * 3];
    for (int i = 0; i<256; i++)
    {
        if (i <= 130)
        {
            lutData[i * 3] = 0;
            lutData[i * 3 + 1] = 0;
            lutData[i * 3 + 2] = 0;
        }
        else if (i > 130 && i <= 210)
        {
            lutData[i * 3] = 255;
            lutData[i * 3 + 1] = 255;
            lutData[i * 3 + 2] = 255;
        }
        else if (i > 210 && i <= 230)
        {
            lutData[i * 3] = 240;
            lutData[i * 3 + 1] = 128;
            lutData[i * 3 + 2] = 128;
        }

        else if (i > 230)
        {
               lutData[i * 3] = 205;
               lutData[i * 3 + 1] = 38;
               lutData[i * 3 + 2] = 38;
        }
        else
        {
            lutData[i * 3] = 255;
            lutData[i * 3 + 1] = 255;
            lutData[i * 3 + 2] = 255;
        }
    }
    Mat lut(1, 256, CV_8UC3, lutData);
    LUT(temImage, lut, temImage);
}

#endif

int Pipe_Merge::get_vmin_value()
{
    int vmin = 0;
    if(g_cameraBriCon.fcBrightness<=30)
    {
        vmin = 180;//v分量取下限 180
    }
    else if(g_cameraBriCon.fcBrightness>30&&g_cameraBriCon.fcBrightness<=60 )
    {
        vmin = 190;//v分量取下限 180
    }
    else if(g_cameraBriCon.fcBrightness>60&&g_cameraBriCon.fcBrightness<=90 )
    {
        vmin = 200;//v分量取下限 180
    }
    else if(g_cameraBriCon.fcBrightness>90&&g_cameraBriCon.fcBrightness<=110 )
    {
        vmin = 220;//v分量取下限 180
    }
    else if(g_cameraBriCon.fcBrightness>110&&g_cameraBriCon.fcBrightness<=127 )
    {
        vmin = 240;//v分量取下限 180
    }
    else if(g_cameraBriCon.fcBrightness>127&&g_cameraBriCon.fcBrightness<=157 )
    {
        vmin = 132;//v分量取下限 180
    }
    else if(g_cameraBriCon.fcBrightness>157&&g_cameraBriCon.fcBrightness<=220 )
    {
        vmin = 150;//v分量取下限 180
    }
    else
    {
         vmin = 170;
    }

    return vmin;
}

void Pipe_Merge::start(Capture *cap0,Capture *cap1,struct drm_plane *plane){
    printf("pipe start\r\n");
    Pipe_Merge_State now=get_status();
    if(now==pm_init){
        this->plane=plane;
        this->cap0=cap0;
        this->cap1=cap1;
        pipeThread=new std::thread(&Pipe_Merge::merge_thread_run,this);
    }
    else if(now==pm_stoped){
        this->plane=plane;
        this->cap0=cap0;
        this->cap1=cap1;
        re_run();
    }
    else{
        fprintf(stderr,"pipe merge already start\r\n");
    }
}
void Pipe_Merge::start_alone(Capture *cap0,Capture *cap1,struct drm_plane *plane,bool  is_inf){
    printf("pipe start\r\n");
    Pipe_Merge_State now=get_status();
    if(now==pm_init){
        this->plane=plane;
        this->cap0=cap0;
        this->cap1=cap1;
        if(is_inf){
            pipeThread=new std::thread(&Pipe_Merge::merge_inf_thread_run,this);
        }
        else{
            pipeThread=new std::thread(&Pipe_Merge::merge_wg_thread_run,this);
        }
    }
    else if(now==pm_stoped){
        this->plane=plane;
        this->cap0=cap0;
        this->cap1=cap1;
        re_run();
    }
    else{
        fprintf(stderr,"pipe merge already start\r\n");
    }
}

#if 0
void Pipe_Merge::merge_thread_run()
{
    struct timeval last={0,0},current={0,0};
      double fps = 0;
      int count=0;
    unsigned char* databuff0;
    int datalen0;
    unsigned char* databuff1;
    int datalen1;


    g_mixPosition.x_hzOffset_hw = 110;
    g_mixPosition.y_verOffset_hw = 20;

    g_cameraBriCon.fcBrightness =157 ;
    g_cameraBriCon.fcContrast =140;
    cout<<"start!";
    Mat out;
    printf("g_mergeMode:%d  gRedColorSwitch:%d\r\n",g_mergeMode,gRedColorSwitch);
    bool already_run_flag=false;

    set_status(pm_runing);
    while(true){
        if(get_status()==pm_runing){
            if(plane->bo==NULL){
                fprintf(stderr,"plane id :%d ->bo is NULL erro,plane not init",plane->plane_id);
                sleep(1);
                continue;
            }
            gettimeofday(&current,NULL);
            //current.tv_sec * 1000 + current.tv_usec / 1000;
            fps = (current.tv_sec - last.tv_sec) * 1000000
                + current.tv_usec - last.tv_usec;
            fps = fps ? 1000000.0 / fps : 0.0;
    //computer start
            Frame tmp0=cap0->getLastFrame();
            Frame tmp1=cap1->getLastFrame();
            databuff0=tmp0.data;
            datalen0   =  tmp0.height*tmp0.width*tmp0.channels/tmp0.split;
            databuff1=tmp1.data;
            datalen1   =  tmp1.height*tmp1.width*tmp1.channels/tmp1.split;

            struct timeval tv1;
            gettimeofday(&tv1,NULL);

            cv::Mat yuv1(HEIGHT*3/2, WIGHT, CV_8UC1);//yuv1.data =databuff0;
            memcpy(yuv1.data,databuff0, datalen0);
            cv::Mat yuv2(HEIGHT*3/2, WIGHT, CV_8UC1);//yuv2.data=databuff1;
            memcpy(yuv2.data,databuff1, datalen1);

            cv::Mat rgb1;
            cv::Mat rgb2;
            cvtColor(yuv1, rgb1, CV_YUV2RGB_NV12);
            cvtColor(yuv2, rgb2, CV_YUV2RGB_NV12);

            struct timeval tv;
            gettimeofday(&tv,NULL);
    //            printf("second:%ld\n",tv.tv_sec);  //秒
    //            printf("millisecond:%ld\n",tv.tv_sec*1000 + tv.tv_usec/1000);  //毫秒
    //            printf("microsecond:%ld\n",tv.tv_sec*1000000 + tv.tv_usec);  //微秒
    //            printf("microsecond  -:%ld\n",tv.tv_sec*1000000 + tv.tv_usec-(tv1.tv_sec*1000000 + tv1.tv_usec));  //微秒
    //            printf("millisecond --:%ld\n",tv.tv_sec*1000 + tv.tv_usec/1000-(tv1.tv_sec*1000 + tv1.tv_usec/1000));  //毫秒
    ////            qDebug()<<"g_mergeMode:"<<g_mergeMode;
    ///
            Pipe_MERGEMODE tmp_mergeMode=getMergemode();
            bool tmp_RedColorSwitch=getColormode();
            if(tmp_mergeMode==Pipe_HONGWAI)
            {
    //                /*伪彩*/qDebug()<<"tmp_RedColorSwitch:"<<tmp_RedColorSwitch;
                if(tmp_RedColorSwitch)
                {
                    //HSV图像
                    cv::Mat hsv;
                    cvtColor(rgb1, hsv, COLOR_RGB2HSV);
                    //输出图像
                    cv::Mat dst, mask;
                    dst = Mat::zeros(rgb1.size(), rgb1.type());
                    inRange(hsv, Scalar(H_MIN, S_MIN,get_vmin_value()),
                            Scalar(H_MAX, S_MAX, V_MAX), mask);
                    bitwise_and(rgb1, rgb1, dst, mask);
                    draw_redcolor_hongwai(dst);

                    Mat rgbhong, outhong;
                    rgbhong = Mat::zeros(rgb1.size(), rgb1.type());
                    outhong = Mat::zeros(rgb1.size(), rgb1.type());
                    rgbhong = rgb1-dst;
                    outhong =(rgbhong*1.0+dst*1.0);
                    resize(outhong,outhong,Size(plane->w,plane->h));
//                    plane_fb_cpy(plane,outhong.data,outhong.cols*outhong.rows*outhong.elemSize());
                    plane_double_fb_cpy(plane,outhong.data,outhong.cols*outhong.rows*outhong.elemSize());
                }
                else
                {
                    resize(rgb1,rgb1,Size(plane->w,plane->h));
//                    plane_fb_cpy(plane,rgb1.data,rgb1.cols*rgb1.rows*rgb1.elemSize());
                    plane_double_fb_cpy(plane,rgb1.data,rgb1.cols*rgb1.rows*rgb1.elemSize());
                    usleep(20*1000);
                }
            }
            else if (tmp_mergeMode==Pipe_WEIGUANG)
            {
                resize(rgb2,rgb2,Size(plane->w,plane->h));
//                plane_fb_cpy(plane,rgb2.data,rgb2.cols*rgb2.rows*rgb2.elemSize());
                plane_double_fb_cpy(plane,rgb2.data,rgb2.cols*rgb2.rows*rgb2.elemSize());
                usleep(20*1000);
            }
            else if(tmp_mergeMode==Pipe_MERGE)
            {
                cv::Mat dstImage;
                //HSV图像
                cv::Mat hsv;
                cvtColor(rgb1, hsv, COLOR_RGB2HSV);
                //输出图像
                cv::Mat dst, mask;
                dst = Mat::zeros(rgb1.size(), rgb1.type());
                inRange(hsv, Scalar(H_MIN, S_MIN,get_vmin_value()),
                        Scalar(H_MAX, S_MAX, V_MAX), mask);
                bitwise_and(rgb1, rgb1, dst, mask);
                cvtColor(dst, dst, CV_RGB2GRAY);
                //尺寸调整
                resize(dst,dstImage,Size(dst.cols/1.5,dst.rows/1.1),0,0,INTER_NEAREST);
                Mat temImage1, temImage2;
                temImage1 = Mat::zeros(dst.size(), dst.type());
                Mat imageROI = temImage1(Rect(g_mixPosition.x_hzOffset_hw,
                                              g_mixPosition.y_verOffset_hw,
                                              dstImage.cols, dstImage.rows));

                dstImage.copyTo(imageROI);
                cvtColor(temImage1, temImage2, CV_GRAY2RGB);
                /*伪彩*/
                if(tmp_RedColorSwitch)
                {
                    draw_redcolor(temImage2);
                }
    //                out = Mat::zeros(rgb2.size(), rgb2.type());
                out =(rgb2*0.8+temImage2*1.0);
    //                qDebug()<<"outsize:"<<sizeof (out.data);
    //                qDebug()<<QString::fromLocal8Bit("第%1次").arg(cnt)<<"outdatalen0:"<<out.size;
                resize(out,out,Size(plane->w,plane->h));
//                plane_fb_cpy(plane,out.data,out.cols*out.rows*out.elemSize());
                plane_double_fb_cpy(plane,out.data,out.cols*out.rows*out.elemSize());
            }
    //        usleep(20*1000);
            count++;
            if(count%60==0){
//            printf("merge merge plane:%d :fps%f\n",plane->plane_id,fps);
        //        fflush(stdout);
            }
            last=current;
            already_run_flag=true;
        }
        else{
            if(already_run_flag){
                already_run_flag=false;
                set_status(pm_stoped);
            }
            usleep(100*1000);
        }
    }
    printf("pipe merge critical erro");
    return;
}

#else
void Pipe_Merge::merge_thread_run()
{
    struct timeval last={0,0},current={0,0};
    double fps = 0;
    int count=0;
    unsigned char* databuff0;
    int datalen0;
    unsigned char* databuff1;
    int datalen1;

    prctl(PR_SET_NAME,"pipe_merge");


    g_mixPosition.x_hzOffset_hw = 110;
    g_mixPosition.y_verOffset_hw = 20;

    g_cameraBriCon.fcBrightness =157 ;
    g_cameraBriCon.fcContrast =140;

    Mat out;
    printf("g_mergeMode:%d  gRedColorSwitch:%d\r\n",g_mergeMode,gRedColorSwitch);
    bool already_run_flag=false;

    set_status(pm_runing);
    while(true)
    {
        if( get_status() == pm_runing )
        {
//            if(plane->bo == nullptr){
//                fprintf(stderr,"plane id :%d ->bo is NULL erro,plane not init",plane->plane_id);
//                usleep(100*1000); //100ms
//                continue;
//            }
            gettimeofday(&current,nullptr);
            fps = (current.tv_sec - last.tv_sec) * 1000000
                + current.tv_usec - last.tv_usec;
            fps = fps ? 1000000.0 / fps : 0.0;

            if(cap0->getSensorSta() == true && cap1->getSensorSta() == true)
            {
                Frame tmp0=cap0->getLastFrame();
                Frame tmp1=cap1->getLastFrame();
                databuff0=tmp0.data;
                datalen0   =  tmp0.height*tmp0.width*tmp0.channels/tmp0.split;
                databuff1=tmp1.data;
                datalen1   =  tmp1.height*tmp1.width*tmp1.channels/tmp1.split;

                cv::Mat yuv1(HEIGHT*3/2, WIGHT, CV_8UC1);//yuv1.data =databuff0;
                cv::Mat yuv2(HEIGHT*3/2, WIGHT, CV_8UC1);//yuv2.data=databuff1;

                memcpy(yuv1.data,databuff0, datalen0);
                memcpy(yuv2.data,databuff1, datalen1);

                cv::Mat rgb1;
                cv::Mat rgb2;
                cvtColor(yuv1, rgb1, CV_YUV2RGB_NV12);
                cvtColor(yuv2, rgb2, CV_YUV2RGB_NV12);
//                resize(rgb1,rgb1,Size(plane->w,plane->h),INTER_NEAREST);
//                resize(rgb2,rgb2,Size(plane->w,plane->h),INTER_NEAREST);

#if 0
               string fileName1;
               string fileName2;
               static int cnt = 0;
               fileName1 = "hongwai_" + to_string(cnt) + ".png";
               fileName2 = "weiguang_" + to_string(cnt) + ".png";
               imwrite(fileName1, rgb1);
               imwrite(fileName2, rgb2);
                      cnt++;
#endif

                Pipe_MERGEMODE tmp_mergeMode=getMergemode();
                bool tmp_RedColorSwitch=getColormode();
                if(tmp_mergeMode == Pipe_HONGWAI)
                {
                    if(tmp_RedColorSwitch) { //红伪彩开关
                        //HSV图像
                        cv::Mat hsv;
                        cvtColor(rgb1, hsv, COLOR_RGB2HSV);
                        //输出图像
                        cv::Mat dst, mask;
                        dst = Mat::zeros(rgb1.size(), rgb1.type());
                        inRange(hsv, Scalar(H_MIN, S_MIN,get_vmin_value()),
                                Scalar(H_MAX, S_MAX, V_MAX), mask);
                        bitwise_and(rgb1, rgb1, dst, mask);
                        draw_redcolor_hongwai(dst);

                        Mat rgbhong, outhong;
                        rgbhong = Mat::zeros(rgb1.size(), rgb1.type());
                        outhong = Mat::zeros(rgb1.size(), rgb1.type());
//?????
                        rgbhong = rgb1-dst;
//                        outhong =(rgbhong*1.0+dst*1.0);
                        outhong =(rgb1*0.8+dst*1.0);
//                        send_uvc_data_to_streambuf(CH_FRONT,outhong.data,WIGHT*HEIGHT*3);
                        resize(outhong,outhong,Size(plane->w,plane->h));
                        plane_fb_cpy(plane,outhong.data, (plane->w)*(plane->h) *3);
                    }else{
//                        send_uvc_data_to_streambuf(CH_FRONT,rgb1.data,WIGHT*HEIGHT*3);
                        resize(rgb1,rgb1,Size(plane->w,plane->h));
                        plane_fb_cpy(plane,rgb1.data, (plane->w) * (plane->h) *3);
                    }
                }else if (tmp_mergeMode==Pipe_WEIGUANG){
//                    send_uvc_data_to_streambuf(CH_FRONT,rgb2.data,WIGHT*HEIGHT*3);
                    resize(rgb2,rgb2,Size(plane->w,plane->h));
                    plane_fb_cpy(plane,rgb2.data, (plane->w)*(plane->h) *3);

                }else if(tmp_mergeMode==Pipe_MERGE){
                    cv::Mat dstImage;
                    //HSV图像
                    cv::Mat hsv;
                    cvtColor(rgb1, hsv, COLOR_RGB2HSV);
                    //输出图像
                    cv::Mat dst, mask;
                    dst = Mat::zeros(rgb1.size(), rgb1.type());
                    inRange(hsv, Scalar(H_MIN, S_MIN,get_vmin_value()), Scalar(H_MAX, S_MAX, V_MAX), mask);
                    bitwise_and(rgb1, rgb1, dst, mask);
                    cvtColor(dst, dst, CV_RGB2GRAY);
                    //尺寸调整
                    resize(dst,dstImage,Size(dst.cols/1.5,dst.rows/1.1),0,0,INTER_NEAREST);
                    Mat temImage1, temImage2;
                    temImage1 = Mat::zeros(dst.size(), dst.type());
                    Mat imageROI = temImage1(Rect(g_mixPosition.x_hzOffset_hw,
                                                  g_mixPosition.y_verOffset_hw,
                                                  dstImage.cols, dstImage.rows));

                    dstImage.copyTo(imageROI);
                    cvtColor(temImage1, temImage2, CV_GRAY2RGB);
                    /*伪彩*/
                    if(tmp_RedColorSwitch){
                        draw_redcolor(temImage2);
                    }
//                    out =(rgb2*0.8+temImage2*1.0);
                    out =temImage2;
//                    send_uvc_data_to_streambuf(CH_FRONT,out.data,WIGHT*HEIGHT*3);
                    resize(out,out,Size(plane->w,plane->h));
                    plane_fb_cpy(plane,out.data, (plane->w)*(plane->h)*3);
                }
                count++;
                if(count%60==0){
                    std:cout<<"rgb1 size"<<rgb1.size<<std::endl;
                    printf("merge merge plane:%d :fps%f\n",plane->plane_id,fps);
                    fflush(stdout);
                }
                last=current;
                already_run_flag=true;
            }
//            usleep(20*1000);
        }else{
            if(already_run_flag){
                already_run_flag=false;
                set_status(pm_stoped);
            }
            usleep(100*1000);
        }
    }
    printf("pipe merge critical erro");
    return;
}

void Pipe_Merge::merge_wg_thread_run()
{
    struct timeval last={0,0},current={0,0};
    double fps = 0;
    int count=0;
    unsigned char* databuff0;
    int datalen0;
    unsigned char* databuff1;
    int datalen1;

    prctl(PR_SET_NAME,"pipe_merge");


    g_mixPosition.x_hzOffset_hw = 110;
    g_mixPosition.y_verOffset_hw = 20;

    g_cameraBriCon.fcBrightness =157 ;
    g_cameraBriCon.fcContrast =140;

    Mat out;
    Mat tmp(720,1280,CV_8UC3);
    printf("g_mergeMode:%d  gRedColorSwitch:%d\r\n",g_mergeMode,gRedColorSwitch);
    bool already_run_flag=false;

    set_status(pm_runing);
    while(true)
    {
        if( get_status() == pm_runing )
        {
//            if(plane->bo == nullptr){
//                fprintf(stderr,"plane id :%d ->bo is NULL erro,plane not init",plane->plane_id);
//                usleep(100*1000); //100ms
//                continue;
//            }
            gettimeofday(&current,nullptr);
            fps = (current.tv_sec - last.tv_sec) * 1000000
                + current.tv_usec - last.tv_usec;
            fps = fps ? 1000000.0 / fps : 0.0;

            if(cap0->getSensorSta() == true && cap1->getSensorSta() == true)
            {
                Frame tmp0=cap0->getLastFrame();
                Frame tmp1=cap1->getLastFrame();
                databuff0=tmp0.data;
                datalen0   =  tmp0.height*tmp0.width*tmp0.channels/tmp0.split;
                databuff1=tmp1.data;
                datalen1   =  tmp1.height*tmp1.width*tmp1.channels/tmp1.split;

                cv::Mat yuv1(HEIGHT*3/2, WIGHT, CV_8UC1);//yuv1.data =databuff0;
                cv::Mat yuv2(HEIGHT*3/2, WIGHT, CV_8UC1);//yuv2.data=databuff1;

                memcpy(yuv1.data,databuff0, datalen0);
                memcpy(yuv2.data,databuff1, datalen1);

                cv::Mat rgb1;
                cv::Mat rgb2;
                cvtColor(yuv1, rgb1, CV_YUV2RGB_NV12);
                cvtColor(yuv2, rgb2, CV_YUV2RGB_NV12);

#if 0
               string fileName1;
               string fileName2;
               static int cnt = 0;
               fileName1 = "hongwai_" + to_string(cnt) + ".png";
               fileName2 = "weiguang_" + to_string(cnt) + ".png";
               imwrite(fileName1, rgb1);
               imwrite(fileName2, rgb2);
                      cnt++;
#endif

                Pipe_MERGEMODE tmp_mergeMode=getMergemode();
                bool tmp_RedColorSwitch=getColormode();
                if(tmp_mergeMode == Pipe_HONGWAI)
                {
                    if(tmp_RedColorSwitch) { //红伪彩开关
                        //HSV图像
                        cv::Mat hsv;
                        cvtColor(rgb1, hsv, COLOR_RGB2HSV);
                        //输出图像
                        cv::Mat dst, mask;
                        dst = Mat::zeros(rgb1.size(), rgb1.type());
                        inRange(hsv, Scalar(H_MIN, S_MIN,get_vmin_value()),
                                Scalar(H_MAX, S_MAX, V_MAX), mask);
                        bitwise_and(rgb1, rgb1, dst, mask);
                        draw_redcolor_hongwai(dst);

                        Mat rgbhong, outhong;
                        rgbhong = Mat::zeros(rgb1.size(), rgb1.type());
                        outhong = Mat::zeros(rgb1.size(), rgb1.type());
//?????
//                        rgbhong = rgb1-dst;
//                        outhong =(rgbhong*1.0+dst*1.0);
                        outhong =(rgb1*0.8+dst*1.0);
//                        send_uvc_data_to_streambuf(CH_FRONT,outhong.data,WIGHT*HEIGHT*3);
                        resize(outhong,outhong,Size(plane->w,plane->h));
                        plane_fb_cpy(plane,outhong.data, (plane->w)*(plane->h) *3);
                    }else{
//                        send_uvc_data_to_streambuf(CH_FRONT,rgb1.data,WIGHT*HEIGHT*3);
                        resize(rgb1,rgb1,Size(plane->w,plane->h));
                        plane_fb_cpy(plane,rgb1.data, (plane->w) * (plane->h) *3);
                    }
                }else if (tmp_mergeMode==Pipe_WEIGUANG){
//                    send_uvc_data_to_streambuf(CH_FRONT,rgb2.data,WIGHT*HEIGHT*3);
                    resize(rgb2,rgb2,Size(plane->w,plane->h));
                    plane_fb_cpy(plane,rgb2.data, (plane->w)*(plane->h) *3);

                }else if(tmp_mergeMode==Pipe_MERGE){

//                    inf_buffer.copyTo(tmp);
//                    /*伪彩*/
//                    if(tmp_RedColorSwitch){
//                        draw_redcolor(tmp);
//                    }
//                    out =(rgb2*0.8+tmp*1.0);

//                    send_uvc_data_to_streambuf(CH_FRONT,rgb2.data,WIGHT*HEIGHT*3);
                    resize(rgb2,rgb2,Size(plane->w,plane->h));
//                    rgb2=0.8*rgb2;
//                    resize(out,out,Size(plane->w,plane->h));
                    plane_fb_cpy(plane,rgb2.data, (plane->w)*(plane->h) *3);
                }
                count++;
                if(count%60==0){
//                    std:cout<<"rgb1 size"<<rgb1.size<<std::endl;
//                    printf("merge merge plane:%d :fps%f\n",plane->plane_id,fps);
//                    fflush(stdout);
                }
                last=current;
                already_run_flag=true;
            }
            usleep(20*1000);
        }else{
            if(already_run_flag){
                already_run_flag=false;
                set_status(pm_stoped);
            }
            usleep(100*1000);
        }
    }
    printf("pipe merge critical erro");
    return;
}

void Pipe_Merge::merge_inf_thread_run()
{
    struct timeval last={0,0},current={0,0};
    double fps = 0;
    int count=0;
    unsigned char* databuff0;
    int datalen0;
    unsigned char* databuff1;
    int datalen1;

    prctl(PR_SET_NAME,"pipe_merge");


    g_mixPosition.x_hzOffset_hw = 110;
    g_mixPosition.y_verOffset_hw = 20;

    g_cameraBriCon.fcBrightness =157 ;
    g_cameraBriCon.fcContrast =140;

    int fp=0;
    struct fb_var_screeninfo vinfo;
    long screensize=0;
    char *fbp = NULL;

    fp = open("/dev/fb0", O_RDWR);

    if(fp < 0) {
        printf("Error : Can not open framebuffer device/n");
    }

    if(ioctl(fp, FBIOGET_VSCREENINFO, &vinfo)){
        printf("Error reading variable information/n");
    }
    screensize = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;

    fbp =(char *)mmap(0, screensize*2, PROT_READ | PROT_WRITE, MAP_SHARED, fp,0);
    printf("Get virt mem = %p\n", fbp);

    Mat out;
    printf("g_mergeMode:%d  gRedColorSwitch:%d\r\n",g_mergeMode,gRedColorSwitch);
    bool already_run_flag=false;

    set_status(pm_runing);
    while(true)
    {
        if( get_status() == pm_runing )
        {
//            if(plane->bo == nullptr){
//                fprintf(stderr,"plane id :%d ->bo is NULL erro,plane not init",plane->plane_id);
//                usleep(100*1000); //100ms
//                continue;
//            }
            gettimeofday(&current,nullptr);
            fps = (current.tv_sec - last.tv_sec) * 1000000
                + current.tv_usec - last.tv_usec;
            fps = fps ? 1000000.0 / fps : 0.0;

            if(cap0->getSensorSta() == true && cap1->getSensorSta() == true)
            {
                Frame tmp0=cap0->getLastFrame();
                Frame tmp1=cap1->getLastFrame();
                databuff0=tmp0.data;
                datalen0   =  tmp0.height*tmp0.width*tmp0.channels/tmp0.split;
                databuff1=tmp1.data;
                datalen1   =  tmp1.height*tmp1.width*tmp1.channels/tmp1.split;

                cv::Mat yuv1(HEIGHT*3/2, WIGHT, CV_8UC1);//yuv1.data =databuff0;
                cv::Mat yuv2(HEIGHT*3/2, WIGHT, CV_8UC1);//yuv2.data=databuff1;

                memcpy(yuv1.data,databuff0, datalen0);
//                memcpy(yuv2.data,databuff1, datalen1);

                cv::Mat rgb1;
                cv::Mat rgb2;
                cvtColor(yuv1, rgb1, CV_YUV2RGB_NV12);
//                cvtColor(yuv2, rgb2, CV_YUV2RGB_NV12);

//                resize(rgb1,rgb1,Size(960,720));
                plane->w=640;
                plane->h=655;
                resize(rgb1,rgb1,Size(plane->w,plane->h));

                cv::Mat yy(HEIGHT, WIGHT, CV_8UC1);//yuv2.data=databuff1;
                memcpy(yy.data,databuff0, HEIGHT*WIGHT);
//                resize(yy,yy,Size(plane->w,plane->h));
                resize(yy,yy,Size(plane->w,plane->h));

#if 0
               string fileName1;
               string fileName2;
               static int cnt = 0;
               fileName1 = "hongwai_" + to_string(cnt) + ".png";
               fileName2 = "weiguang_" + to_string(cnt) + ".png";
               imwrite(fileName1, rgb1);
               imwrite(fileName2, rgb2);
                      cnt++;
#endif
                bool tmp_RedColorSwitch=getColormode();
                    cv::Mat dstImage;
                    cv::Mat dst, mask;
                    //HSV图像
                    cv::Mat hsv;
//                    cvtColor(rgb1, hsv, COLOR_RGB2HSV);
                    //输出图像
//                    dst = Mat::zeros(rgb1.size(), rgb1.type());
//                    inRange(hsv, Scalar(H_MIN, S_MIN,get_vmin_value()), Scalar(H_MAX, S_MAX, V_MAX), mask);
//                    bitwise_and(rgb1, rgb1, dst, mask);
//                    cvtColor(dst, dst, CV_RGB2GRAY);

//gwj new
#if 1
                    cvtColor(yy, dst, CV_GRAY2BGR);
                    /*伪彩*/
                    if(tmp_RedColorSwitch){
                        draw_redcolor(dst);
                    }
//                    dst=3*dst;
//                    plane_fb_cpy(plane,dst.data, (plane->w)*(plane->h)*3);
                    for(int i=0;i<dst.rows;i++){
                        memcpy(fbp+1920*3*(i+20)+590*3,dst.data+dst.cols*3*i,dst.cols*3);
                    }

#endif
                    //尺寸调整
//                    resize(dst,dstImage,Size(dst.cols/1.5,dst.rows/1.1),0,0,INTER_NEAREST);
//                    Mat temImage1, temImage2;
//                    temImage1 = Mat::zeros(dst.size(), dst.type());
//                    Mat imageROI = temImage1(Rect(g_mixPosition.x_hzOffset_hw,
//                                                  g_mixPosition.y_verOffset_hw,
//                                                  dstImage.cols, dstImage.rows));

//                    dstImage.copyTo(imageROI);
//                    cvtColor(temImage1, temImage2, CV_GRAY2RGB);
//gwj::global value mat
//                    cvtColor(temImage1, inf_buffer, CV_GRAY2RGB);

//                    /*伪彩*/
//                    if(tmp_RedColorSwitch){
//                        draw_redcolor(temImage2);
//                    }
//                    out =(rgb2*0.8+temImage2*1.0);

//                    send_uvc_data_to_streambuf(CH_FRONT,out.data,WIGHT*HEIGHT*3);
////                    resize(out,out,Size(plane->w,plane->h));
//                    plane_fb_cpy(plane,out.data, (plane->w)*(plane->h)*3);
//                    RGB_to_NV12_intrinsic(temImage2.data,buffer,plane->w,plane->h);
//                    plane_double_fb_cpy(plane,buffer,plane->w*plane->h*3/2);

                count++;
                if(count%180==0){
                    std:cout<<"rgb1 size"<<rgb1.size<<"type:"<<rgb1.type()<<std::endl;
                    printf("merge merge plane:%d :fps%f\n",plane->plane_id,fps);
                    fflush(stdout);
                }
                last=current;
                already_run_flag=true;
            }
//            usleep(20*1000);
        }else{
            if(already_run_flag){
                already_run_flag=false;
                set_status(pm_stoped);
            }
            usleep(100*1000);
        }
    }
    printf("pipe merge critical erro");
    return;
}

#endif

